import random
import time
from robomaster import robot
from robomaster import conn
from multi_robomaster import multi_robot
from robomaster import led, blaster
from collections import deque

import pos_nokov as pos
import logging

import sympy as sp
import math
import msvcrt
from interval import Interval
from position import *
from utils import *

done=False
# field_error=False
# field_range=[[-1.33,3.04],[-1.50,1.70]]
aim_x=Interval(-1.15, 3.00)
aim_y=Interval(-1.30,1.50)
safe_x=Interval(-1.32,2.76)
safe_y=Interval(-1.42,1.61)
goal=(-1.2,0)
power=0
trans_pt=pose(0.05,0.05,5)
pt=pose()

ROLES=['chase','catch','pass','def','shoot']
bp=0 # ball possesion，持球agent的id
passing=False
sb=deque()



def led_red_solid(robot_group):
    """红灯常亮"""
    robot_group.led.set_led(led.COMP_ALL, 255, 1, 1, led.EFFECT_ON)

def led_grey_solid(robot_group):
    """grey常亮"""
    robot_group.led.set_led(led.COMP_ALL, 75, 255, 255, led.EFFECT_ON)
    
def get_dphi(phi_s, phi_t):
    '''from phi_s to phi_t, turn how much angle'''
    phi=phi_t-phi_s
    return limit_pi(phi)

def calc_catching_pose(b, t, aim=False, catch_phase1=False):
    '''Calc pose to catch ball. b: ball pose, t: target pose'''
    x,y=sp.symbols('x, y')
    distance=0.316 if catch_phase1 else 0.41 # distance from ball to pose
    # logging.debug(f'dist:{distance}')
    solutions=sp.nonlinsolve([(x-b.x)*(b.y-t.y)-(y-b.y)*(b.x-t.x),
                              (x-b.x)**2+(y-b.y)**2-distance**2],[x,y])
    for s in solutions:
        if aim: # the catching pose aim at t
            if not (s[0] in Interval(b.x, t.x) and s[1] in Interval(b.y,t.y)):
                return pose(s[0],s[1],math.atan2(t.y-b.y, t.x-b.x)*180/math.pi)
        else: # t is agent pose and catch directly
            # logging.debug(f'sx, {s[0]},sy: {s[1]}; bx: {b.x}, by: {b.y}; tx: {t.x}, ty: {t.y}')
            if (s[0]-b.x)*(t.x-b.x)>0 and (s[1]-b.y)*(t.y-b.y)>0:
                # logging.debug(f'solved pt: x:{s[0]},y:{s[1]}')
                return pose(s[0],s[1],math.atan2(b.y-t.y, b.x-t.x)*180/math.pi)          

def calc_closest_pose_on_v(p,b,v):
    '''
    计算p在向量v（球速度）上的投影点, v经过点b（球）
    pt: 投影点的位置,速度角度的反方向（机器人应当接球的方向)
    '''
    global trans_pt,pt
    tx,ty=sp.symbols('tx, ty')
    s=sp.solve([(tx-b.x)*v.y-v.x*(ty-b.y), v.x*(tx-p.x)+v.y*(ty-p.y)],[tx,ty])
    pt=pose(s[tx],s[ty])
    tphi=limit_pi(math.atan2(v.y,v.x)*180/math.pi+180-p.phi)
    logging.debug(f'tphi:{tphi}, p.phi:{p.phi}')
    # logging.info(f'dynamic v1 catch pt: x: {pt.x}, y:{pt.y}, phi:{tphi+p.phi}')
    trans_pt=trans_relative_co(p,pt)
    trans_pt.update(trans_pt.x,trans_pt.y,tphi)
    # logging.info(f'dynamic v1 catch trans_pt: x: {trans_pt.x}, y:{trans_pt.y}, phi:{trans_pt.phi}')
    # return trans_pt
    
def catch_mobile(p, b, v):
    '''
    给定球的位置b、速度v、机器人位置p，决定是否夹爪
    return: bool
    '''
    logging.info('judging....')
    # logging.debug(f'vx:{v.x},vy:{v.y}')
    tpred=get_len(b.x-p.x,b.y-p.y)/get_len(v.x,v.y)
    lerror=abs((b.x-p.x)*v.y-(b.y-p.y)*v.x)
    logging.debug(tpred) # 预计碰撞时间
    logging.debug(lerror) # 共线则接近0
    if lerror<0.01 and tpred<0.2:
        return True
    
class EP:
    def __init__(self, multi_robots, robot_group, id, role='stand_by'):
        global sb
        self.role=role
        self.multi_robots=multi_robots
        self.catch_phase=0
        self.close_to_oppo=False
        self.robot_group=robot_group
        self.id=id
        self.aim_to_pass=True
        if self.role=='stand_by':
            sb.append(self.id)
        
    def init_arm(self, rg):
        rg.robotic_arm.moveto(0, -70).wait_for_completed()
        rg.gripper.open(100)
        time.sleep(2)
        
    def stop(self, rg):
        rg.chassis.drive_wheels(w1=0, w2=0, w3=0, w4=0,timeout=1)
        time.sleep(0.01)
    
    def chase(self, rg):
        sf=0.9 # speed adjust factor
        global trans_pt,sb,pt
        p=pos.p
        # 如果self.ready_to_catch为真，则以包住球为target pose
        if self.aim_to_pass: # chase while aiming the stand_by teammate
            logging.info('chase aiming')
            pt=calc_catching_pose(p.pb, p[sb[0]], True, self.catch_phase==1)
        else: # chase directly
            logging.info('chase directly')
            pt=calc_catching_pose(p.pb, p[self.id], False, self.catch_phase==1)
        trans_pt=trans_relative_co(p[self.id], pt) # 获得机器人坐标系下的目标坐标pt
        trans_pt.update(trans_pt.x, trans_pt.y, limit_pi(pt.phi-p[self.id].phi))
        rg.chassis.drive_speed(math.tanh(trans_pt.x/1.5), -math.tanh(trans_pt.y), -30*(math.tanh(trans_pt.phi/40)), timeout=1)
        time.sleep(0.1)
        
    def catch_still(self, rg):
        global bp
        p=pos.p
        rg.gripper.close(100)
        time.sleep(0.3)
        if check_catched(p[self.id],p.pb):
            logging.info('catched')
            self.catch_phase=3
            bp=self.id
            t1=time.time()
            rg.robotic_arm.moveto(x=0, y=-60).wait_for_completed()
            t2=time.time()
            logging.debug(f'arm lift time:{t2-t1: .3f} s')
        else:
            logging.info('catch failed')
            self.catch_phase=1
            rg.gripper.open(100)
            time.sleep(0.31) 
    
    def pass_ball(self,rg):
        global passing,bp
        p=pos.p
        t2=time.time()
        dphi=limit_pi(p[self.id].phi-math.atan2(p[sb[0]].y-p[self.id].y, p[sb[0]].x-p[self.id].x)*180/math.pi)
        logging.debug(f'dphi to pass:{dphi}')
        if abs(dphi)>2:
            rg.chassis.drive_speed(0,0,30*(math.tanh(dphi/10)))
            time.sleep(0.1)
        else:
            rg.chassis.drive_speed(0,0,0)
            time.sleep(0.01)
            
            rg.chassis.drive_speed(5,0,0,timeout=2) # shoot
            time.sleep(0.4)
            # rg.robotic_arm.moveto(0,5).wait_for_completed() # move up arm
            passing=True
            rg.gripper.open(power=100)
            # rg.chassis.drive_speed(1.3,0,0) # shoot
            time.sleep(0.4)
            rg.chassis.drive_speed(0,0,0) # stop
            time.sleep(0.01)
            t3=time.time()
            logging.debug('time for passing: % .3f s' % (t3-t2))
            self.catch_phase=0
            bp=0
            passing=False
            self.role='stand_by'
            sb.append(self.id)      
            rg.chassis.move(x=-1.4,xy_speed=3).wait_for_completed()
            rg.robotic_arm.moveto(0, -70).wait_for_completed()
        
    def catch(self, rg):
        global trans_pt,bp
        p=pos.p
        v=pos.v
        vtemp=speed(math.cos(p[bp].phi),math.sin(p[bp].phi))
        calc_closest_pose_on_v(p[self.id],p.pb,v.vb)
        logging.debug('dynamic catch')
        if catch_mobile(p[self.id],p.pb,v.vb):
            rg.gripper.close(power=100)
            if check_catched(p[self.id],p.pb):
                logging.info('catched')
                self.catch_phase=3
                bp=self.id
                rg.robotic_arm.moveto(x=0, y=-60).wait_for_completed()
            else:
                logging.info('catch failed')
                self.catch_phase=1
                rg.gripper.open(100)
                time.sleep(0.31) 
        rg.chassis.drive_speed(math.tanh(trans_pt.x/2), -math.tanh(trans_pt.y), -30*(math.tanh(trans_pt.phi/30)), timeout=1)
        time.sleep(0.2)
        
    def run(self,event):
        logging.basicConfig(level=logging.DEBUG)
        global trans_pt,passing,done,pt, sb
        v=pos.v
        self.multi_robots.run([self.robot_group, self.init_arm])
        event.wait()
        slow_c=0
        while(not pos.done):
            p=pos.p
            try:
                # if pt.x in safe_x and pt.y in safe_y:
                #     self.aim_to_pass=True
                # else:
                self.aim_to_pass=False
                    # logging.warning(f'stop! unsafe pt: {pt.x}, {pt.y}')
                    # break
                if self.role=='chase':
                    # if get_len(v.vb.x, v.vb.y)>0.3:
                    #     logging.info(f'vb:{v.vb}')
                    #     self.role='catch'
                    if self.catch_phase!=3:
                        if self.catch_phase!=2:
                            self.multi_robots.run([self.robot_group, self.chase])
                            if self.catch_phase==0:
                                # logging.debug('chase, x:%.3f, y:%.3f, phi:%.2f' % (trans_pt.x,trans_pt.y,trans_pt.phi))
                                if -0.15<trans_pt.x<0.15 and abs(trans_pt.y)<0.05 and abs(trans_pt.phi)<2:
                                    logging.info('catch phase 1')
                                    self.catch_phase=1
                            elif abs(trans_pt.x)<0.015 and abs(trans_pt.y)<0.015 and abs(trans_pt.phi)<2:
                                logging.info('catch phase 2')
                                self.catch_phase=2
                        else:
                            self.multi_robots.run([self.robot_group, self.catch_still])
                    # elif not self.close_to_oppo:
                    #     self.multi_robots.run([self.robot_group, self.attack])
                    else:
                        self.multi_robots.run([self.robot_group, self.pass_ball])
                        
                elif self.role=='stand_by':
                    self.multi_robots.run([self.robot_group, self.stop])
                    # logging.debug(f'stand_by:{self.id}')
                    if passing:
                        logging.info(f'stand_by list:{sb}')
                        self.multi_robots.run([self.robot_group, self.stop])
                        self.role='catch'
                        sb.popleft()
                        logging.info(f'stand_by list:{sb}')
                elif self.role=='catch':
                    logging.info(f'vb:{v.vb}')
                    logging.debug(f'count:{slow_c}')
                    if get_len(v.vb.x, v.vb.y)>0.4:
                        slow_c=0
                    else:
                        slow_c+=1
                        time.sleep(0.05)
                    if slow_c<2:
                        self.multi_robots.run([self.robot_group, self.catch])
                    else:
                        self.role='chase'
                        self.catch_phase=0
            except Exception as e:
                print(f'Exception: {e}')
                print('seee')  
                break
            
            if (msvcrt.kbhit() and msvcrt.getch() == b'p'):
                print("ep %d Paused..." % self.id)
                while msvcrt.getch() != b'r':
                    time.sleep(0.5)
                    msvcrt.getch()
                print("ep %d resumed..." % self.id)
            if msvcrt.kbhit() and msvcrt.getch() == b't':
                break    
        done=True
        print('over...')
        self.multi_robots.close()
